 /**
 * MKS SERVO42B
 * Copyright (c) 2020 Makerbase. 
 *
 * Based on nano_stepper project by Misfittech
 * Copyright (C) 2018  MisfitTech LLC.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */
 
#include "sine.h"

static const int16_t sineTable[1280] = {
0,3,6,9,13,16,19,22,25,28,31,34,38,41,44,47,
50,53,56,59,63,66,69,72,75,78,81,84,87,90,94,97,
100,103,106,109,112,115,118,121,124,127,130,133,136,139,142,145,
148,151,154,157,160,163,166,169,172,175,178,181,184,187,190,193,
196,198,201,204,207,210,213,216,218,221,224,227,230,233,235,238,
241,244,246,249,252,255,257,260,263,265,268,271,273,276,279,281,
284,286,289,292,294,297,299,302,304,307,309,312,314,317,319,322,
324,327,329,331,334,336,338,341,343,345,348,350,352,355,357,359,
361,364,366,368,370,372,374,377,379,381,383,385,387,389,391,393,
395,397,399,401,403,405,407,409,410,412,414,416,418,420,421,423,
425,427,428,430,432,433,435,437,438,440,441,443,445,446,448,449,
451,452,454,455,456,458,459,461,462,463,465,466,467,468,470,471,
472,473,474,476,477,478,479,480,481,482,483,484,485,486,487,488,
489,490,491,492,492,493,494,495,496,496,497,498,499,499,500,501,
501,502,502,503,503,504,505,505,505,506,506,507,507,508,508,508,
509,509,509,509,510,510,510,510,510,511,511,511,511,511,511,511,
511,511,511,511,511,511,511,511,510,510,510,510,510,509,509,509,
509,508,508,508,507,507,506,506,505,505,505,504,503,503,502,502,
501,501,500,499,499,498,497,496,496,495,494,493,492,492,491,490,
489,488,487,486,485,484,483,482,481,480,479,478,477,476,474,473,
472,471,470,468,467,466,465,463,462,461,459,458,456,455,454,452,
451,449,448,446,445,443,441,440,438,437,435,433,432,430,428,427,
425,423,421,420,418,416,414,412,410,409,407,405,403,401,399,397,
395,393,391,389,387,385,383,381,379,377,374,372,370,368,366,364,
361,359,357,355,352,350,348,345,343,341,338,336,334,331,329,327,
324,322,319,317,314,312,309,307,304,302,299,297,294,292,289,286,
284,281,279,276,273,271,268,265,263,260,257,255,252,249,246,244,
241,238,235,233,230,227,224,221,218,216,213,210,207,204,201,198,
196,193,190,187,184,181,178,175,172,169,166,163,160,157,154,151,
148,145,142,139,136,133,130,127,124,121,118,115,112,109,106,103,
100,97,94,90,87,84,81,78,75,72,69,66,63,59,56,53,
50,47,44,41,38,34,31,28,25,22,19,16,13,9,6,3,
0,-3,-6,-9,-13,-16,-19,-22,-25,-28,-31,-34,-38,-41,-44,-47,
-50,-53,-56,-59,-63,-66,-69,-72,-75,-78,-81,-84,-87,-90,-94,-97,
-100,-103,-106,-109,-112,-115,-118,-121,-124,-127,-130,-133,-136,-139,-142,-145,
-148,-151,-154,-157,-160,-163,-166,-169,-172,-175,-178,-181,-184,-187,-190,-193,
-196,-198,-201,-204,-207,-210,-213,-216,-218,-221,-224,-227,-230,-233,-235,-238,
-241,-244,-246,-249,-252,-255,-257,-260,-263,-265,-268,-271,-273,-276,-279,-281,
-284,-286,-289,-292,-294,-297,-299,-302,-304,-307,-309,-312,-314,-317,-319,-322,
-324,-327,-329,-331,-334,-336,-338,-341,-343,-345,-348,-350,-352,-355,-357,-359,
-361,-364,-366,-368,-370,-372,-374,-377,-379,-381,-383,-385,-387,-389,-391,-393,
-395,-397,-399,-401,-403,-405,-407,-409,-410,-412,-414,-416,-418,-420,-421,-423,
-425,-427,-428,-430,-432,-433,-435,-437,-438,-440,-441,-443,-445,-446,-448,-449,
-451,-452,-454,-455,-456,-458,-459,-461,-462,-463,-465,-466,-467,-468,-470,-471,
-472,-473,-474,-476,-477,-478,-479,-480,-481,-482,-483,-484,-485,-486,-487,-488,
-489,-490,-491,-492,-492,-493,-494,-495,-496,-496,-497,-498,-499,-499,-500,-501,
-501,-502,-502,-503,-503,-504,-505,-505,-505,-506,-506,-507,-507,-508,-508,-508,
-509,-509,-509,-509,-510,-510,-510,-510,-510,-511,-511,-511,-511,-511,-511,-511,
-511,-511,-511,-511,-511,-511,-511,-511,-510,-510,-510,-510,-510,-509,-509,-509,
-509,-508,-508,-508,-507,-507,-506,-506,-505,-505,-505,-504,-503,-503,-502,-502,
-501,-501,-500,-499,-499,-498,-497,-496,-496,-495,-494,-493,-492,-492,-491,-490,
-489,-488,-487,-486,-485,-484,-483,-482,-481,-480,-479,-478,-477,-476,-474,-473,
-472,-471,-470,-468,-467,-466,-465,-463,-462,-461,-459,-458,-456,-455,-454,-452,
-451,-449,-448,-446,-445,-443,-441,-440,-438,-437,-435,-433,-432,-430,-428,-427,
-425,-423,-421,-420,-418,-416,-414,-412,-410,-409,-407,-405,-403,-401,-399,-397,
-395,-393,-391,-389,-387,-385,-383,-381,-379,-377,-374,-372,-370,-368,-366,-364,
-361,-359,-357,-355,-352,-350,-348,-345,-343,-341,-338,-336,-334,-331,-329,-327,
-324,-322,-319,-317,-314,-312,-309,-307,-304,-302,-299,-297,-294,-292,-289,-286,
-284,-281,-279,-276,-273,-271,-268,-265,-263,-260,-257,-255,-252,-249,-246,-244,
-241,-238,-235,-233,-230,-227,-224,-221,-218,-216,-213,-210,-207,-204,-201,-198,
-196,-193,-190,-187,-184,-181,-178,-175,-172,-169,-166,-163,-160,-157,-154,-151,
-148,-145,-142,-139,-136,-133,-130,-127,-124,-121,-118,-115,-112,-109,-106,-103,
-100,-97,-94,-90,-87,-84,-81,-78,-75,-72,-69,-66,-63,-59,-56,-53,
-50,-47,-44,-41,-38,-34,-31,-28,-25,-22,-19,-16,-13,-9,-6,-3,
0,3,6,9,13,16,19,22,25,28,31,34,38,41,44,47,
50,53,56,59,63,66,69,72,75,78,81,84,87,90,94,97,
100,103,106,109,112,115,118,121,124,127,130,133,136,139,142,145,
148,151,154,157,160,163,166,169,172,175,178,181,184,187,190,193,
196,198,201,204,207,210,213,216,218,221,224,227,230,233,235,238,
241,244,246,249,252,255,257,260,263,265,268,271,273,276,279,281,
284,286,289,292,294,297,299,302,304,307,309,312,314,317,319,322,
324,327,329,331,334,336,338,341,343,345,348,350,352,355,357,359,
361,364,366,368,370,372,374,377,379,381,383,385,387,389,391,393,
395,397,399,401,403,405,407,409,410,412,414,416,418,420,421,423,
425,427,428,430,432,433,435,437,438,440,441,443,445,446,448,449,
451,452,454,455,456,458,459,461,462,463,465,466,467,468,470,471,
472,473,474,476,477,478,479,480,481,482,483,484,485,486,487,488,
489,490,491,492,492,493,494,495,496,496,497,498,499,499,500,501,
501,502,502,503,503,504,505,505,505,506,506,507,507,508,508,508,
509,509,509,509,510,510,510,510,510,511,511,511,511,511,511,511
};

int32_t sine(uint16_t angle)
{
	return sineTable[angle];
}

int32_t cosine(uint16_t angle)
{
	angle = angle + (SINE_STEPS >> 2);
	return sineTable[angle];
}

int32_t fastAbs(int32_t v)
{
	int32_t t;
	t = v >> 31;
	return (v ^ t) - t;
}
